
import socket
import json
import copy
import time

import yaml
import os
import traceback

import numpy as np

import rospy
from geometry_msgs.msg import Point
from acupoint_msgs.msg import AcupointCoordinate,LegCoordinate
from std_msgs.msg import Bool
from std_msgs.msg import Int64

M=np.matrix([[0,    -1, 0,  -9.5],
             [1,    0,  0,  -408.5],
             [0,    0,  1,  0],
             [0,    0,  0,  1]])
inv_M=np.matrix([[0,    1,  0,   408.5],
                 [-1,   0,  0,   -9.5],
                 [0,    0,  1,   0],
                 [0,    0,  0,   1]])

class JakaCommunicate:
    def __init__(self):
        # self.actions=self.loadYaml('./temp.yaml')
        self.actions=self.loadYaml('./exp2.yaml')
        self.agv_postions=self.parseAgvposition()
        # temp=self.getInverseMatrix(M)
        # print(temp)
        # print(self.agv_postions)
        
        
        
       
        # self.getOtherAgvPosition()
        # return
        if self.actions is None:
            print("Error, load yaml file failed")
            return
        if self.agv_postions is None:
            print("Error, load yaml file failed")
            return
        print("parse completed")
        # print(self.agv_postions)
         

        
        self.actions=self.actions['actions']
        self.deg2rad=3.1415926/180
        self.flag=-1
        self.acupoint_name=None
        self.acupoint_coordinate=None
        self.left_coordinate=[0,0,0]
        self.right_coordinate=[0,0,0]
        
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu2base_coordinate", AcupointCoordinate, self.acupointCallback) 
        self.left_leg_sub = rospy.Subscriber("/acupoint/left_leg_line", LegCoordinate, self.leftLegCallback) 
        self.right_leg_sub = rospy.Subscriber("/acupoint/right_leg_line", LegCoordinate, self.rightLegCallback) 
        self.jaka_arrive_pub=rospy.Publisher("/jaka_arrive",Bool,queue_size=1)
        self.agv_pub=rospy.Publisher("/agv/agv_position",Int64,queue_size=1)
        self.agv_sub=rospy.Subscriber("/agv/agv_arrive", Bool, self.agvCallback) 
        
        
        
        rospy.sleep(1)
        
        self.client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.client.connect(('192.168.1.106',56789))
        # data={'method':10}
        # data_json=json.dumps(data)        
        # self.client.send(data_json.encode())
        # self.client.settimeout(600)
        # recv_data=self.client.recv(1024)
        
        print("move agv to the start point")
        self.agv_flag=False
        agv_msg=Int64()
        agv_msg.data=1
        self.agv_pub.publish(agv_msg)
        # print(self.agv_index)
        while not rospy.is_shutdown() and not self.agv_flag:
            rospy.sleep(0.1)
        self.agv_flag=False
        self.agv_index=0
        print("agv has been in the start point")


        while self.acupoint_coordinate is None or self.left_coordinate[0]==0 or self.right_coordinate[0]==0:
            rospy.sleep(0.1)
        print("action start")
        
        x=-1
        y=-1
        z=-1
        
        try:
            
            index=0
            massage_actions=[]
            
            try:
                temp=[]
                for action in self.actions:
                    method=action['method']
                    
                    if method==-1:
                        massage_actions.append(temp)
                        temp=[]
                    else:
                        temp.append(action)
                # print(massage_actions) 
                for massage_action in massage_actions:
                    self.agvMove(massage_action)
                    self.jakaMove(massage_action)
            except:
                print("error during actions")
                print(traceback.format_exc())
        except:
            print("connect failed")
            print(traceback.format_exc())
        finally:
            pass
            self.client.close()
    
    def getInverseMatrix(self,R):
        inv_T=np.matrix([[1,0,0,0],
                         [0,1,0,0],
                         [0,0,1,0],
                         [0,0,0,1.0]])
        inv_T[0:3,0:3]=R[0:3,0:3].T;
        inv_T[0:3,3]=-R[0:3,0:3].T*R[0:3,3]
        return inv_T
    
    def parseAgvposition(self):
        temp=self.loadYaml('./agv_position.yaml')

        if temp is None:
            print("Error, load yaml file failed")
            return []
        positions=temp['agv_points']
        agv_positions=[]
        for position in positions:
            agv_positions.append(self.quantern2RotationMatrix(position))
        return agv_positions
        
    def quantern2RotationMatrix(self, point):
        x=point['orientation']['x']
        y=point['orientation']['y']
        z=point['orientation']['z']
        w=point['orientation']['w']
        temp=np.matrix([[1-2*y*y-2*z*z,   2*x*y+2*w*z,      2*x*z-2*w*y,    point['position']['x']],
                           [2*x*y-2*w*z,     1-2*x*x-2*z*z,    2*y*z+2*w*x,    point['position']['y']],
                           [2*x*z+2*w*y,     2*y*z-2*w*x,      1-2*x*x-2*y*y,  point['position']['z']],
                           [0,               0,                0,              1]]);
        return temp
        
        
    def getOtherAgvPosition(self):
        other_position=[]
        position_now=self.getInverseMatrix(self.agv_postions[self.agv_index])
        # print(position_now*self.agv_postions[self.agv_index])
        for position in self.agv_postions:
            temp=M*position_now*position*inv_M
            # print(temp)
            other_position.append([temp[0,3],temp[1,3],temp[2,3]])
            
        
        return other_position
    
    def agvCallback(self,agv_arrive):
        self.agv_flag=agv_arrive.data
    
    def jakaMove(self,massage_action):
        x=-1
        y=-1
        z=-1
        for action in massage_action:
            method=action['method']
            if method==3:
                if action['name']=='left_leg':
                    x=self.left_coordinate[0]
                    y=self.left_coordinate[1]
                    z=self.left_coordinate[2]
                elif action['name']=='right_leg':
                    x=self.right_coordinate[0]
                    y=self.right_coordinate[1]
                    z=self.right_coordinate[2]
                else:
                    print("undefined name in tweak")
            else:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x
                y=self.acupoint_coordinate[index].y
                z=self.acupoint_coordinate[index].z
            data={'method':action['method'],'time':action['time'],'x':x,'y':y,'z':z}
            data_json=json.dumps(data)
            # print(data_json)
            # wait_key = raw_input(">")
            wait_key='y'
            if wait_key=='y':
                self.client.send(data_json.encode())
                self.client.settimeout(600)
                recv_data=self.client.recv(1024)
                if not recv_data:
                    break
                jaka_arrived_msg=Bool()
                jaka_arrived_msg=True
                self.jaka_arrive_pub.publish(jaka_arrived_msg)
            else:
                pass
        
    def agvMove(self,massage_action):
        coordinates=[]
        x_list=[]
        y_list=[]
        z_list=[]
        for action in massage_action:
            method=action['method']
            if method==0:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x+180
                y=self.acupoint_coordinate[index].y
                z=self.acupoint_coordinate[index].z+120
                rx=-90*self.deg2rad
                ry=-20*self.deg2rad
                rz=90*self.deg2rad
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz])
            elif method==1:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x+180
                y=self.acupoint_coordinate[index].y+30
                z=self.acupoint_coordinate[index].z+175
                rx=105*self.deg2rad
                ry=0*self.deg2rad
                rz=-90*self.deg2rad
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz])
            elif method==2:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x+130
                y=self.acupoint_coordinate[index].y
                z=self.acupoint_coordinate[index].z+125
                rx=-90*self.deg2rad
                ry=-90*self.deg2rad
                rz=90*self.deg2rad
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz]) 
            elif method==3:
                if action['name']=='left_leg':
                    x=self.left_coordinate[0]
                    y=self.left_coordinate[1]
                    z=self.left_coordinate[2]
                elif action['name']=='right_leg':
                    x=self.right_coordinate[0]
                    y=self.right_coordinate[1]
                    z=self.right_coordinate[2]
                else:
                    print("undefined name in tweak")
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
        # print(x_list)
        x_min=min(x_list)
        x_max=max(x_list)
        y_min=min(y_list)
        y_max=max(y_list)
        z_min=min(z_list)
        z_max=max(z_list)
        x=(x_min+x_max)/2
        y=(y_min+y_max)/2
        z=(z_min+z_max)/2
        print(x,y,z)
        min_distance=0.9*(x*x+y*y+z*z)
        min_index=self.agv_index
        agv_positions=self.getOtherAgvPosition()
        print(agv_positions)
        for i in range(len(agv_positions)):
            d=(agv_positions[i][0]-x)**2+(agv_positions[i][1]-y)**2+(agv_positions[i][2]-z)**2
            if d<min_distance:
                min_index=i
                min_distance=d
        print(min_index)
        if min_index!=self.agv_index:
            data={'method':10}
            data_json=json.dumps(data)
            # print(data_json)
            # wait_key = raw_input(">")
            wait_key='y'
            if wait_key=='y':
                self.client.send(data_json.encode())
                self.client.settimeout(600)
                recv_data=self.client.recv(1024)
            agv_msg=Int64()
            agv_msg.data=min_index+1
            
            self.agv_flag=False
            self.agv_pub.publish(agv_msg)
            # print(self.agv_index)
            while not rospy.is_shutdown() and not self.agv_flag:
                rospy.sleep(0.1)
            
            self.agv_flag=False
            self.agv_index=min_index
            
            
      
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_coordinate=copy.deepcopy(msg).acupoints
        # print("u are in callback")
    
    
    def leftLegCallback(self,msg):
        self.left_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.left_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.left_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
    
    def rightLegCallback(self,msg):
        self.right_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.right_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.right_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
        
    def loadYaml(self,file_path):
        if not os.path.exists(file_path):
            print("Error, %s not exists"%file_path)
            return
        yaml_data=None
        try:
            with open(file_path) as f:
                yaml_data=yaml.safe_load(f.read())
        except:
            print("Some error occured,the exception message is:")
            print(traceback.format_exc())
        return yaml_data


if __name__ == "__main__":
    rospy.init_node('nuc_communicate')
    jc=JakaCommunicate()



    

    
